#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>  // 包含VoxelGrid滤波器的头文件
#include <iostream>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pcd_to_occ");
    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/occ_map_out", 1); // 这个是转换后的地图话题，把这个给到fastplanner就行
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;  // 用于存储降采样后的点云
    sensor_msgs::PointCloud2 output;

    // 加载PCD文件
    pcl::io::loadPCDFile("/home/cxr/drone_ws/src/pcd_pkg/1109-outdoor.pcd", cloud); // 在这里修改你的pcd格式的地图路径，然后保存编译运行就行

    // 创建VoxelGrid滤波器对象
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(cloud.makeShared());  // 设置输入点云
    voxel_grid.setLeafSize(0.1f, 0.1f, 0.1f);  // 设置体素大小，单位为米。可以根据需要调整这个值
    voxel_grid.filter(cloud_downsampled);  // 执行滤波，结果存储在cloud_downsampled中

    // 将降采样后的点云转换为ROS消息
    pcl::toROSMsg(cloud_downsampled, output);
    output.header.frame_id = "world";

    ros::Rate loop_rate(1);
    std::cout << "开始转换" << std::endl;
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}